import math

from RPi import GPIO


class Drive:
    pwm_freq = 10000

    def __init__(self):
        """
        gpio 初始化
        """
        GPIO.setmode(GPIO.BCM)
        # 引脚定义
        # 将pwm引脚设置为输出
        self.PWMA_PIN = 18
        self.PWMB_PIN = 23
        GPIO.setup([self.PWMB_PIN, self.PWMA_PIN], GPIO.OUT, initial=GPIO.LOW)
        self.PWMA = GPIO.PWM(self.PWMA_PIN, Drive.pwm_freq)
        self.AIN2 = 27
        self.AIN1 = 22
        self.PWMB = GPIO.PWM(self.PWMB_PIN, Drive.pwm_freq)
        self.BIN2 = 24
        self.BIN1 = 25

        # 设置输出模式
        GPIO.setup([self.AIN2, self.AIN1, self.BIN2, self.BIN1], GPIO.OUT, initial=GPIO.LOW)
        # 启动pwm
        self.PWMA.start(0)
        self.PWMB.start(0)

    def stop(self):
        """
        小车停止
        :return:
        """
        # PWM 停止
        self.PWMA.start(0)
        self.PWMB.start(0)

    def left(self, speed):
        """
        控制左侧轮子速度
        :param speed:
        :return:
        """
        assert -100 <= speed <= 100, f"速度值({speed})超出了范围"
        if speed > 0:
            GPIO.output(self.AIN2, 0)
            GPIO.output(self.AIN1, 1)
        else:
            GPIO.output(self.AIN2, 1)
            GPIO.output(self.AIN1, 0)
        self.PWMA.ChangeDutyCycle(abs(speed))

    def right(self, speed):
        """
        控制右侧轮子速度
        :param speed:
        :return:
        """
        assert -100 <= speed <= 100, f"速度值({speed})超出了范围"
        if speed > 0:
            GPIO.output(self.BIN2, 0)
            GPIO.output(self.BIN1, 1)
        else:
            GPIO.output(self.BIN2, 1)
            GPIO.output(self.BIN1, 0)
        self.PWMB.ChangeDutyCycle(abs(speed))

    def move(self, direction: float, speed: float):
        """
        小车
        :param direction: 方向
        :param speed:  速度
        :return:
        """
        speed *= 99
        # 与车身垂直方向速度
        a = math.tan(direction) * speed
        # 与车身平行方向速度
        b = speed
        # 计算左侧速度分量
        left = b - a / 2
        self.left(min(left, 99) if left >= 0 else max(left, -99))
        # 计算右侧速度分量
        right = b + a / 2
        self.right(min(right, 99) if right >= 0 else max(right, -99))

    def __del__(self):
        """
        析构函数，清空所有引脚的配置
        :return:
        """
        self.PWMA.stop()
        self.PWMB.stop()
        GPIO.cleanup()


if __name__ == '__main__':
    import time

    drive = Drive()
    drive.left(100)
    time.sleep(2)
    drive.right(100)
    time.sleep(2)
    drive.stop()
    drive.left(50)
    drive.right(50)
    time.sleep(3)
